/************************************************
   TELEMETRE ULTRASON POUR L'ASSERV EN ALTITUDE
***********************************************/
#include "../libDCM/libDCM.h"
#include "libUDB_internal.h"
#include <stdio.h>

unsigned int altitude = 0;
unsigned int altitudePrecMes = 0;
unsigned int rised = 0;

void US_Init(void)
{
    TMR4 = 0;
    PR4 = 0xFFFF;
    T4CON = 0b0000000000100000;
    _TRISD15 = 1 ;
    CNEN2bits.CN21IE = 1;
    T4CONbits.TON = 1;
    IFS1bits.CNIF = 0;
    IEC1bits.CNIE = 1;
    IPC4bits.CNIP = 7;
}	

void __attribute__((__interrupt__,__no_auto_psv__)) _CNInterrupt(void)
{
	indicate_loading_inter ;
	interrupt_save_set_corcon ;	
	
	unsigned int time ;
	IFS1bits.CNIF = 0; // clear the interrupt
	time = TMR4;
	if (PORTDbits.RD15)
	{
            rised = time ;
	}
	else
	{
            altitude = time - rised;
            altitude *=3;
            altitude /= 10;
            altitude += 30;
            if(abs(altitude-altitudePrecMes)<500){udb_flags._.telemeter_updated = 1; altitudePrecMes = altitude;}
	}
	
	interrupt_restore_corcon ;
	return ;
}
